#!/usr/bin/env python
# -*- encoding: UTF-8 -*-
from __future__ import print_function

import cv2
import numpy as np

from naoqi import ALProxy
import vision_definitions
import motion
from nao import Nao

class Sim(object):
    def initialization(self):
        self.red = Nao("127.0.0.1", 9559)
        self.blue = Nao("127.0.0.1", 9560)
        self.frame = motion.FRAME_TORSO # 使用的机器人参考系
        self.goToInitPose()
        self.loadParams()

    def run(self):
        prv_line = None
        while True:
            self.show_img()
            line = self.get_line()
            if self.line_neq(line,prv_line):
                self.track_line(line)
            prv_line = line
            if cv2.waitKey(-1) == 27: break
        cv2.destroyAllWindows()

    def goToInitPose(self):
        # TODO: 两机器人摆到初始位姿
        self.red.simulationInitPose()
        self.blue.simulationInitPose()

    def loadParams(self):
        # TODO: 加载相机内参数,
        # 注意: 标定时使用的棋盘格为 9x4 单元格大小为 1cm
        # 仿真时使用的棋盘格为 9x4 单元格大小为 1.5cm 与实际要使用的棋盘格同
        # 相机矩阵
        self.mtx = self.get_cameraMatrix()
        self.dist = self.get_distCoeffs()

    def get_cameraMatrix(self):
        mtx = np.eye(3)
        fx = 268.811991582
        fy = 268.791086901
        cx = 155.578363269
        cy = 117.351365465
        mtx[0,0],mtx[1,1],mtx[0,2],mtx[1,2] = fx,fy,cx,cy
        return mtx

    def get_distCoeffs(self):
        dist = np.zeros((1,5))
        k1 = 0.0150941632039
        k2 = -0.108440142617
        p1 = -0.00247220924849
        p2 = -0.00306933857966
        k3 = 0.158224608925
        dist[0] = k1,k2,p1,p2,k3
        return dist

    def get_line(self):
        # TODO: 重建三维空间直线
        pass

    def line_neq(self,l1,l2):
        # TODO: 判断两直线是否相同
        pass

    def track_line(self,line):
        # TODO: 指定物体跟踪直线,与直线成固定位姿
        pass

    def show_img(self):
        cv2.imshow("red_top",self.red.get_top_image())
        cv2.imshow("red_bottom",self.red.get_bottom_image())
        cv2.imshow("blue_top",self.blue.get_top_image())
        cv2.imshow("blue_bottom",self.blue.get_bottom_image())

sim = Sim()
sim.initialization()
sim.run()
